// main.c
// Josh
// standard libs
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

#include "main.h"
#include "ff_utils.h"
#include "../utils/LCD_functions.h"
#include "../utils/util.h"
#include "../utils/button.h"

extern volatile char KEY_VALID;


// line sensors are connected to ADC4(PF4) thru ADC7(PF7).  Port F is also known as the JTAG port and 
// JTAG must be disabled inorder to use these pins.  The pins are not numbered the same as the other connectors.
//                              +---+---+---+---+---+
//            Port F  (JTAG)    | X | X | X | X | X |  Pins marked with an X should not be used.  
//            Usable Pins       +---+---+---+---+---+
//                              | 7 | X | 5 | 6 | 4 |  
//                              +---+---+---+---+---+
//PF4 - RightDistSensor
//PF6 - MiddleDistSensor
//PF5 - LeftDistSensor 	
//PF7 - FrontLineSensor 

// Motor control circuit is connected to PortB  
//                              +---+---+---+---+---+
//            Port B            | 1 | 3 | 5 | X | V |  Pins marked with an X are not be used.  
//     Motor Contorl pins       +---+---+---+---+---+
//                              | 0 | 2 | x | x | G |  
//                              +---+---+---+---+---+
//  PB0 - Left motor  pin 2 on IXDN404 (yellow)
//  PB1 - Right motor pun 2 on IXDN404 (blue)
//  PB2 - Left motor  pin 4 on IXDN404 (green)
//  PB3 - Right motor pin 4 on IXDN404 (orange)

//	PB5 - pin 6, 0x20, balloon pop sensor


int main (void)
{ 
    Initialization();   //
	USART_Init(51);
	I2C_Init();			// Inits I2C Routines
    SetupLineSensors();
    InitADC();
	LCD_puts_f(PSTR("FF v3.02"),1);
	sendString("Fire Fight ");
	TimerWait(5);
	sendString("Version ");
	TimerWait(5);
	sendString("3.02");
    TimerWait(5);	
	
	unsigned char	OriginalHeading;
	OriginalHeading = 0x7A;
    //
	while(1)
	{
	LCD_puts_f(PSTR("Main"),1);
    while (!KEY_VALID);
    if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
			LCD_puts_f(PSTR("Room 1"),1);
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				ToRoom1();
				LCD_puts_f(PSTR("Back"),1);
				TimerWait(500);
			}
			LCD_puts_f(PSTR(" 1 Room 2"),1);
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				ToRoom2();
				LCD_puts_f(PSTR("Back"),1);
				TimerWait(500);
			}	
			LCD_puts_f(PSTR("2 Room 3"),1);
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				ToRoom3();
				LCD_puts_f(PSTR("Back"),1);
				TimerWait(500);
			}
			LCD_puts_f(PSTR("Room 4"),1);
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				ToRoom4();
				LCD_puts_f(PSTR("Back"),1);
				TimerWait(500);
			}	
			LCD_puts_f(PSTR("Exit Room"),1);
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				ExitRoom();
				LCD_puts_f(PSTR("Back"),1);
				TimerWait(500);
			}		
			LCD_puts_f(PSTR("R wall follow"),1);
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				RWallFollow(0x3D,UNTIL_KEYPRESS,1);
				LCD_puts_f(PSTR("Back"),1);
				TimerWait(1000);	
			}	
			LCD_puts_f(PSTR("L wall follow"),1);
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				LWallFollow(0x30,UNTIL_KEYPRESS,1);	
				LCD_puts_f(PSTR("Back"),1);
				TimerWait(1000);	
			}	
			LCD_puts_f(PSTR("L R DistTest"),1);
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				showADCLeftRight();
				LCD_puts_f(PSTR("Back"),1);
				TimerWait(1000);
			}	
			LCD_puts_f(PSTR("Home F 1"),1);
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				HomeFrom1();
				LCD_puts_f(PSTR("Back"),1);
				TimerWait(500);
			}			
			LCD_puts_f(PSTR("Home F 2"),1);
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				HomeFrom2();
				LCD_puts_f(PSTR("Back"),1);
				TimerWait(500);
			}			
			LCD_puts_f(PSTR("Home F 3"),1);
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				HomeFrom3();
				LCD_puts_f(PSTR("Back"),1);
				TimerWait(500);
			}			
			LCD_puts_f(PSTR("Home F 4"),1);
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				HomeFrom4();
				LCD_puts_f(PSTR("Back"),1);
				TimerWait(500);
			}			
        }
	LCD_puts_f(PSTR("Test"),1);
    while (!KEY_VALID);
    if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
			LCD_puts_f(PSTR("Check For Flame"),1);
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				ScanRoomForFlame();
				LCD_puts_f(PSTR("Back"),1);
				TimerWait(500);
			}	
			LCD_puts_f(PSTR("Line C DistTest"),1);
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				showADCLineCenter();
				LCD_puts_f(PSTR("Back"),1);
				TimerWait(1000);
			}	
			LCD_puts_f(PSTR("Go To Flame"),1);
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				GoToFlame();
				LCD_puts_f(PSTR("Back"),1);
				TimerWait(1500);
			}		
			LCD_puts_f(PSTR("Compass Diff"),1);
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				CompassDiff();
				LCD_puts_f(PSTR("Back"),1);
				TimerWait(500);
			}	
			LCD_puts_f(PSTR("Go To Heading Test"),1);
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				HeadingTest();
				LCD_puts_f(PSTR("Back"),1);
				TimerWait(1000);
			}					
			LCD_puts_f(PSTR("Therm Noise Test"),1);
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				ThermNoise();
				LCD_puts_f(PSTR("Back"),1);
			}
			LCD_puts_f(PSTR("Comp Noise Test"),1);
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				CompNoise();
				LCD_puts_f(PSTR("Back"),1);
			}		
			LCD_puts_f(PSTR("Flame Sensor Test"),1);
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				FlameSensorTest();
				LCD_puts_f(PSTR("Back"),1);
				TimerWait(1000);
			}
			LCD_puts_f(PSTR("Compass Test"),1);
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				CompassTest();
				LCD_puts_f(PSTR("Back"),1);
				TimerWait(1000);
			}
			LCD_puts_f(PSTR("Check for flame"),1);
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				ScanRoomForFlame();
				LCD_puts_f(PSTR("Back"),1);
				TimerWait(1500);
			}	
			LCD_puts_f(PSTR("PWM Test"),1);
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				PWM_Test();
				LCD_puts_f(PSTR("Back"),1);
				TimerWait(1500);
			}
		}
	LCD_puts_f(PSTR("Example"),1);
    while (!KEY_VALID);
    if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
			LCD_puts_f(PSTR("Example"),1);
			while (!KEY_VALID);
			if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
			{
				TimerWait(500);		// delay while letting go!
				//Example();
				LCD_puts_f(PSTR("Back"),1);
				TimerWait(1500);
			}
		}
	}
}
